﻿namespace SEED.SEEDControllers {
    using System;

    public class Steim2 {

        public static int[] Decode(byte[] bytes, int numSamples, bool swapBytes, int bias) {
            
            if (bytes.Length % 64 != 0) {
                throw new ArgumentException("encoded data length is not multiple of 64 bytes (" + bytes.Length + ")");
            }

            int[] samples = new int[numSamples];
            int[] tempSamples;
            int numFrames = bytes.Length / 64;
            int current   = 0;
            int start     = 0, end = 0;
            int firstData = 0;
            int lastValue = 0;

            for (int i = 0; i < numFrames; i++) {
                tempSamples = ExtractSamples(bytes, i * 64, swapBytes);

                firstData = 0;              // d(0) is byte 0 by default
                if (i == 0) {               // special case for first frame
                    lastValue = bias;       // assign our X(-1)

                    // x0 and xn are in 1 and 2 spots
                    start = tempSamples[1];  // X(0) is byte 1 for frame 0
                    end = tempSamples[2];    // X(n) is byte 2 for frame 0
                    firstData = 3; // d(0) is byte 3 for frame 0
                    // if bias was zero, then we want the first sample to be X(0) constant
                    if (bias == 0) lastValue = start - tempSamples[3];  // X(-1) = X(0) - d(0)
                }

                for (int j = firstData; j < tempSamples.Length && current < numSamples; j++) {
                    // X(n) = X(n-1) + d(n)
                    samples[current] = lastValue + tempSamples[j];  
                    lastValue = samples[current];
                    current++;
                }
            }
            return samples;
        }

        public static int[] Decode(byte[] bytes, int numSamples, bool swapBytes) {
            // zero-bias version of decode
            return Decode(bytes, numSamples, swapBytes, 0);
        }

        private static int[] ExtractSamples(byte[] bytes, int offset, bool swapBytes) {
           
            int nibbles = Utility.bytesToInt(bytes[offset], bytes[offset + 1], bytes[offset + 2], bytes[offset + 3], swapBytes);
            int currNibble = 0;
            int dnib = 0;
            // 7 samples * 15 long words + 1 nibble int
            int[] temp = new int[106]; 
            int tempInt;
            int currNum = 0;
            for (int i = 0; i < 16; i++) {
                currNibble = (nibbles >> (30 - i * 2)) & 0x03;
                switch (currNibble) {
                    case 0:
                        // only include header info if offset is 0
                        if (offset == 0) {
                            temp[currNum++] = Utility.bytesToInt(bytes[offset + (i * 4)],
                                    bytes[offset + (i * 4) + 1],
                                    bytes[offset + (i * 4) + 2],
                                    bytes[offset + (i * 4) + 3],
                                    swapBytes);
                        }
                        break;
                    case 1:
                        temp[currNum++] = Utility.bytesToInt(bytes[offset + (i * 4)]);
                        temp[currNum++] = Utility.bytesToInt(bytes[offset + (i * 4) + 1]);
                        temp[currNum++] = Utility.bytesToInt(bytes[offset + (i * 4) + 2]);
                        temp[currNum++] = Utility.bytesToInt(bytes[offset + (i * 4) + 3]);
                        break;
                    case 2:
                        tempInt = Utility.bytesToInt(bytes[offset + (i * 4)],
                                bytes[offset + (i * 4) + 1],
                                bytes[offset + (i * 4) + 2],
                                bytes[offset + (i * 4) + 3],
                                swapBytes);
                        dnib = (tempInt >> 30) & 0x03;
                        switch (dnib) {
                            case 1:
                                temp[currNum++] = (tempInt << 2) >> 2;
                                break;
                            case 2:
                                temp[currNum++] = (tempInt << 2) >> 17;  // d0
                                temp[currNum++] = (tempInt << 17) >> 17; // d1
                                break;
                            case 3:
                                temp[currNum++] = (tempInt << 2) >> 22;  // d0
                                temp[currNum++] = (tempInt << 12) >> 22; // d1
                                temp[currNum++] = (tempInt << 22) >> 22; // d2
                                break;
                            default:
                                break;
                        }
                        break;
                    case 3:
                        tempInt = Utility.bytesToInt(bytes[offset + (i * 4)],
                                bytes[offset + (i * 4) + 1],
                                bytes[offset + (i * 4) + 2],
                                bytes[offset + (i * 4) + 3],
                                swapBytes);
                        dnib = (tempInt >> 30) & 0x03;
                        // for case 3, we are going to use a for-loop formulation that
                        // accomplishes the same thing as case 2, just less verbose.
                        int diffCount = 0;  // number of differences
                        int bitSize = 0;    // bit size
                        int headerSize = 0; // number of header/unused bits at top
                        switch (dnib) {
                            case 0:
                                headerSize = 2;
                                diffCount  = 5;
                                bitSize    = 6;
                                break;
                            case 1:
                                headerSize = 2;
                                diffCount  = 6;
                                bitSize    = 5;
                                break;
                            case 2:
                                headerSize = 4;
                                diffCount  = 7;
                                bitSize    = 4;
                                break;
                            default:
                                break;
                        }
                        if (diffCount > 0) {
                            for (int d = 0; d < diffCount; d++) {  // for-loop formulation
                                temp[currNum++] = (tempInt << (headerSize + (d * bitSize))) >> (((diffCount - 1) * bitSize) + headerSize);
                            }
                        }
                        break;
                }
            }

            int[] output = new int[currNum];
            Array.Copy(temp, 0, output, 0, currNum);
            return output;
        }
    }
}
